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Abstract — This paper formulates an optimal control problem 
for a system of rigid bodies that are connected by ball joints 
and immersed in an irrotational and incompressible fluid. The 
rigid bodies can translate and rotate in three-dimensional space, 
and each joint has three rotational degrees of freedom. We 
assume that internal control moments are applied at each joint. 
We present a computational procedure for numerically solving 
this optimal control problem, based on a geometric numerical 
integrator referred to as a Lie group variational integrator. This 
computational approach preserves the Hamiltonian structure of 
the controlled system and the Lie group configuration manifold 
of the connected rigid bodies, thereby finding complex optimal 
maneuvers of connected rigid bodies accurately and efficiently. 
This is illustrated by numerical computations. 

I. Introduction 

Fish locomotion involves a deformable fish body interact- 
ing with an unsteady fluid, through which internal muscular 
forces on the fish are translated into fish motions [1]. 

The planar articulated rigid body model has become popu- 
lar in engineering, as it describes underwater robotic vehicles 
that move and steer by changing their shape [2] . Furthermore, 
if it is assumed that the ambient fluid is incompressible and 
irrotational, then equations of motion of the articulated rigid 
body can be derived without explicitly incorporating fluid 
variables [3]. The effect of the fluid is accounted by added 
inertia terms of the rigid body. This model is known to 
characterize the qualitative behavior of fish swimming [3]. 

In [4], an analytical model and a geometric numerical 
integrator for three-dimensional connected rigid bodies im- 
mersed in an incompressible and iiTotational fluid were 
developed. The connected rigid bodies can freely translate 
and rotate in three-dimensional space, and each joint has 
three rotational degrees of freedom. The geometric numerical 
integrator presented in [4] is referred to as a Lie group 
variational integrator [5], and it preserves the symplecticity, 
momentum map, and Lie group configuration manifold of 
the connected rigid bodies. These properties are important 
for accurately and efficiently studying complex maneuvers 
of rigid bodies [6]. 

This paper formulates an optimal control problem for con- 
nected rigid bodies in a perfect fluid. We assume that internal 
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control moments are applied at each joint. These control 
moments change the relative attitude between rigid bodies, 
thereby controlling the shape of a system of connected 
rigid bodies. By using the nonlinear coupling, referred to 
as a geometric phase [7], between shape changes and group 
motions, and by using the momentum exchange between the 
rigid bodies and the ambient fluid, the system of connected 
rigid bodies can translate and rotate without external forces 
or moments. 

We present a computational approach to solve optimal 
control problems based on the Lie group variational integra- 
tors developed in [4]. The optimal control problems are for- 
mulated as discrete-time optimal control problems using Lie 
group variational integrators; these problems can be solved 
using standard numerical optimization techniques. This is in 
contrast to conventional approaches where discretization is 
introduced at the last stage in order to solve the optimaUty 
conditions numerically. 

This computational geometric optimal control approach 
preserves the Hamiltonian structure of the controlled system 
and Lie group structure of connected rigid bodies in fluid [8]. 
As it does not introduce artificial numerical dissipation that 
typically appear in general-purpose numerical integration 
methods, it improves the overall computational efficiency and 
accuracy of the optimization process. As it is coordinate- 
free, this approach avoids singularities and complexities 
associated with local coordinates. 

Optimal shape changes for a planar articulated body to 
achieve a desired locomotion objective have been studied 
in [9], [10]. The main contribution of this paper is to develop 
a computational framework for studying optimal maneuvers 
of connected rigid bodies in a three-dimensional space. By 
considering rigid body motions in a Lie group, this approach 
can find nontrivial optimal maneuvers involving large three- 
dimensional rotations. 

II. Dynamics of Connected Rigid Bodies 
Immersed in a Perfect Fluid 

Consider three connected rigid bodies immersed in a 
perfect fluid. We assume that these rigid bodies are connected 
by a ball joint that has three rotational degrees of freedom, 
and the fluid is incompressible and irrotational. We also 
assume each body has neutral buoyancy. 

A. Configuration Manifold 

We choose a reference frame and three body-fixed frames. 
The origin of each body-fixed frame is located at the center 
of mass of the rigid body. Define 
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Fig. 1 . Connected rigid bodies immersed in a perfect fluid 

Ri G SO (3) Rotation matrix from the i-th body-fixed 
frame to the reference frame 

Slj G M.^ Angular velocity of the z-th body, repre- 
sented in the i-th body-fixed frame 

a; G M'^ Vector from the origin of the reference 

frame to the center of mass of the 0-th 
body, represented in the reference frame 

Vi G M'^ Velocity of the i-th body represented in 

the i-\h body fixed frame 

dij G M'^ Vector from the center of mass of the «-th 
body to the ball joint connecting the i-th 
body with the j-th body, represented in the 
i-th body-fixed frame 
G M Mass of the i-th body 

jf G M^^^ Inertia matrix of the i-th body 

Ui G Internal control moment applied at the i- 

th joint represented in the 0-th body fixed 
frame 

fori, J G {0,1,2}. 

A configuration of this system can be described by the 
location of the center of mass of the central body, and the 
attitude of each rigid body with respect to the reference 
frame. The configuration manifold is G = SE(3) x S0(3) x 
S0(3), where S0(3) = {i? G M^''^ | R = /, det R = 1}, 
and SE(3) = SO(3)(s)M3. 

The attitude kinematic equation is given by 

Ri — Ri^li 

for i G {0,1,2}, where the hat map • : M"^ ^ so(3) is 
defined by the condition that xy — x x y for any x,y G M'^. 
The velocity of the central body is Vo = RqX- Since the 
location of the center of mass of the i-th rigid body can be 
written as a; + Rodoi — Ridio for i G {1,2} with respect to 
the reference frame, Vi is given by 

V, = Rfi - RfRJo^^o + d^o^,- (1) 
B. Lagrangian 

The total kinetic energy of the connected rigid bodies in 
a perfect fluid is the sum of the kinetic energy of the rigid 



bodies and the kinetic energy of the fluid. As the fluid is 
irrotational, the kinetic energy of the fluid can be expressed 
without explicitly incorporating fluid variables. The effects of 
the ambient fluid is accounted for by added inertia terms [3]. 
The resulting model is shown to capture the qualitative 
properties of the interaction between rigid body dynamics 
and fluid dynamics correctly [3], [9]. 

Let Af/, jf G M.'^^^ be the added inertia matrices for the 
translational kinetic energy and the rotational kinetic energy 
of the i-th rigid body. To simplify expressions for the added 
inertia terms, we assume each body is an ellipsoid. The added 
inertia matrices depends on the length of principal axes, and 
their explicit expressions are available in [11]. 

The total kinetic energy can be written as 

2 1 1 

i=0 

where M, = 771^/3x3 + m/, J, = + j/ for i = {0, 1, 2}. 
This can be written in matrix form as 

T=^eKRo,Ri,R2)^, (3) 

where ^ = [fio; i; ^li; ^12] G K^^ ^nd the matrix 
I(i?o, Ri,R2) G M12X12 sjjj^g potential field, 

this corresponds to the Lagrangian of the connected rigid 
bodies in a perfect fluid. 

C. Euler-Lagrange Equations 

Euler-Lagrange equations for a mechanical system that 
evolves on an arbitrary Lie group are given by 

^D^LCg, - ad* • T>^L{g, " '^li^g ' DgL(.g, = U, 

(4) 

9 = 9^, (5) 

where L : TG ~ G x g ^ M is the Lagrangian of the 
system [5]. Here 'D^L{g,£^) G g* denotes the derivative of 
the Lagrangian with respect to ^ G g, ad* : g x g* ^ g* is 
the co-adjoint operator, and T*Lg : T*G ^ g* denotes the 
cotangent lift of the left translation map, Lg : G G [12], 
restricted to the fiber over the identity e. 

For the configuration manifold G = S0(3) x R'^ x S0(3) x 
S0(3), we left-trivialize TG to yield G x g, and we identify 
its Lie algebra g with IR^^ tjjg hat map. Using this result, 
Euler-Lagrange equations for the uncontrolled dynamics of 
connected rigid bodies in a perfect fluid have been developed 
in [4]. Here, we focus on finding the effect of internal control 
moments represented by ?7 G g* in (01). 

The central 0-th rigid body is subject to the control 
moment ui + U2- Let p G R'^ be the vector from the center 
of mass of the 0-th rigid body to a mass element, and let 
dF{p) G R'^ be the force acting on the mass element ex- 
pressed in the 0-th body fixed frame. Then, J^^^ p x dF{p) = 
U1 + U2- The virtual displacement of the mass element due to 
the rotation of the 0-th body can be written as SRop ~ Rof/op 
in the reference frame, where the variation of the rotation 
matrix is represented by 6R0 = ^ | ^^g^o exp er)o = RoVo 



for rjo G M^. Then, the virtual work done by the control 
moment on the 0-th rigid body is given by 
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{dF{p), fjQp) = ?7o • (P X dF{p)) = 770 • {ui + wa)- 



Similarly, the bodies Bi and B2 are subject to the control 
moments —RJRqUi and —R2R0U2, respectively, with re- 
spect to their body fixed frames. The effect of the control 
moments is given by 



U = [ui+ U2; 03> 



-RiRqui; 



-R2 R0U2] 



(6) 



Substituting (|6]l into dU, and using the results of [4], 
the Euler-Lagrange equations for the connected rigid bodies 
immersed in a perfect fluid are given by 



I{Ro, Ri, R2) 



no 

X 

ill 

fl2 



X Jfln + RlxMoRlx + Y.l=i do^RlR^W^ 
RoinoMo - Mono)R^x + ^^^^ 
fli X Jifli +Vix MiVi - dioWi 

n2 X J2n2+V2X M2V2 - d20W2 



Rq — Rn,i, Ri — Rifti, i?2 — R2^2, 



(7) 
(8) 



where 



K: = RJx - RfR^dor^o + d^n,, (9) 
= {h,M, - M,Cl,){Rjx - RjRodo^no) 



MiRf Roflodo^rio + n.Midion, 



(10) 



for i e {1,2}. 
D. Symmetry 

Let the momentum of the system be ^ = [po; Px', Pi', P2] G 
]^i2 ^ g* -pjjg Legendre transformation can be written 
as p — DjL(g,^) — I(i?o, -Ri, -R2)C- The total linear 
momentum of the connected rigid bodies and the ambient 
fluid is given by Px, and the total angular momentum is 
given by pn = xpx + X^Lo ^iPi ^ 

The Lagrangian for the connected rigid bodies in a perfect 
fluid is left-invariant under rigid translation and rotation of 
the entire system. As a result, the configuration manifold can 
be reduced to a shape space G/SE(3) = S0(3) x S0(3), and 
the total angular momentum and the total linear momentum 
are preserved. 

The control moments ui and U2 change the relative atti- 
tudes between rigid bodies in the shape space S0(3) x S0(3). 
Since they respect the symmetry of the Lagrangian, the total 
linear momentum and the total angular momentum are also 
preserved in the controlled dynamics of the connected rigid 
bodies. 



Geometric numerical integration deals with numerical 
integrators that preserve geometric features of a dynamical 
system, such as invariants, symmetry, and reversibility [13]. 
For numerical simulation of Hamiltonian systems that evolve 
on a Lie group, such as our system of connected rigid 
bodies in a fluid, it is critical to preserve both the sym- 
plectic property of Hamiltonian flows and the Lie group 
structure [6]. A geometric numerical integrator, referred to 
as a Lie group variational integrator, has been developed for 
a system of connected rigid bodies in a perfect fluid [4]. It 
has desirable properties, such as preserving the symplecticity, 
momentum map, and Lie group structure of the system, 
thereby providing qualitatively correct numerical results for 
complex maneuvers over a long time period. 

The computational geometric optimal control approach 
utilizes the structure-preserving properties of geometric inte- 
grators in an optimization process [8], [5]. More explicitly, 
a discrete-time optimal control problem is formulated using 
Lie group variational integrators, and general optimization 
techniques, such as an indirect method based on optimality 
conditions or a direct method based on nonlinear programing, 
are applied. 

This method has substantial computational advantages. 
Since variational integrators compute the energy dissipation 
of a controlled system accurately [14], more accurate op- 
timal trajectories are obtained. Since there is no artificial 
numerical dissipation induced by the integration algorithms, 
this approach is numerically more robust, and the optimal 
control input can be computed efficiently. By representing 
the configuration of the rigid bodies directly on a Lie group, 
this method avoids singularities and complexities that appear 
in local coordinates. These properties are particularly useful 
for connected rigid bodies that are controlled indirectly 
by nonlinear coupling effects through large-angle rotational 
maneuvers. 

A. Lie Group Variational Integrator 

Here, we generalize the Lie group variational integrator 
for a system of connected rigid bodies in a perfect fluid 
developed in [4], to include the effects of the internal control 
moments ui and U2- 

Let h > be a fixed integration step size, and let a 
subscript k denote the value of a variable at the fc-th time 
step. We define a discrete-time kinematic equation as follows. 
Define fk - (Fq, , Axfe, Fi, , F2 J £ G for Axk G M.\ 
Fo^,Fi^,F2^ e S0(3) such that gk+i = gkfk- 

= {Ro,Fo„ Xk + Axk, RuFi„ R2,F2,). (11) 

Therefore, fk represents the relative update between two 
integration steps. This ensures that the structure of the Lie 
group configuration manifold is numerically preserved. 



Discrete Lagmngian: A discrete Lagrangian 
Ld{gk,fk) ■ G X G M is an approximation of the 
Jacobi solution of the Hamilton-Jacobi equation, which is 
given by the integral of the Lagrangian along the exact 
solution of the Euler-Lagrange equations over a single time 
step. The discrete Lagrangian of connected rigid bodies is 
chosen as 

^1 1 



1 

2h 



-AxlR,,M,RlR„,iFo, - I)do, 
-AxlR,,M,{F,~I)dM 



where nonstandard inertia matrices are defined as 



dliFl - I)RlR^,AW, - I)d,o) , (12) 



(13) 



1 



(15) 



4 = 2^''"^^^ ^ ~ dMd^o, (14) 

for i e {1,2}. 

Effects of Control Moments: For a discrete Lagrangian 
defined on an arbitrary Lie group, the following discrete- 
time Euler-Lagrange equations, referred to as a Lie group 
variational integrator, were developed in [5]. 

+ T:l,,^, • Dg.^^i,,^, + [/-^ + Ul_^ = 0, 

= Qkfk, (16) 

where Ad* : G x g* — * g* is the co-Adjoint operator [12]. 
The discrete generalized forces [/^ , UJ^ G g are chosen so 
as to approximate the virtual work of external forces and 
moments over a time step: 

U{t)-rjdt^U^^-jjk + U+-jjk+u 

where the variation of the configuration variable is repre- 
sented by Sg — grj for ?7 G g. 
From (|6]l, these are chosen as 



(17) 



Discrete-time Euler-Lagrange Equations: Substituting 
(fT2] i. ([TtI i into (fTsT l. ( fTSI l. the discrete-time Euler-Lagrange 
equations for the connected rigid bodies immersed in a 
perfect fluid are given by 

+ {MoRl^^Axk+i)''Rl^^Axk+i 

2 



( 7' F -F'^T'V-iF T' - J' F'^ )^ 



dioFl^MiRj^Bi^ + Fik+idioMtRf^^_^Bi^^^ 



Ra.MoR^^Axk + Ai, 



hR^RokUi^, 



Rok+iMoR'o^^^Axk+i - Ai,^, - A2,^, = 0, 

Ofc + i 



Rik + 1 ~ Rik^ik 



Xk+1 =Xk+ Axk, 



(19) 



(20) 

(21) 
(22) 



where inertia matrices are given by ( fT3] l. (fl4l i. and 

Ai^ , Bi^ e ]R3 are given by 

A, = i?,,M, {Rj^B,, - (F,, - I)d,o) , (23) 

B, , = Axk + Ro, {Fo, - I)dQ,. (24) 

For given (go, fo) £ G x G, gi e G is obtained by (I2ni-(l22ll. 
and /i G G is obtained by solving (fT8]l-(|20]i. This yields a 
discrete-time Lagrangian flow map {go, Jo) — > and 
this process is iterated. 

This integrator preserves the symplectic form, momentum 
maps, and Lie group structure of a system of connected rigid 
bodies in a fluid exactly (up to machine precision), and the 
total energy conservation error is uniformly bounded over a 
long-time period when there is no control moment. 

B. Optimal Control Problem 

The objective of the optimal control problem of this paper 
is to transfer the connected rigid bodies from an initial 
configuration to a desired terminal condition during a fixed 
maneuver time Nh, while minimizing the control inputs: 



\k=Q 



■Wife +W2fc ■U2J 



(25) 



(18) 



As discussed before, the control moments ui and U2 
change the relative attitudes between rigid bodies in the 
shape space S0(3) x S0(3), and the total linear momen- 
tum and the total angular momentum are preserved in the 
controlled dynamics of the connected rigid bodies. 

A system of connected rigid bodies can translate and 
rotate as a consequence of the effects of geometric phase 
and momentum exchange between the rigid bodies and the 
ambient fluid. Geometric phase refers to a translation along 
the symmetry direction achieved by closed trajectories in 
the shape space [7]. By controlling the relative attitudes, a 
system of connected rigid bodies can translate and rotate 
without external forces or moments. Based on geometric 
phase effects, the controllability and motion planning al- 
gorithms for articulated multibody systems with reaction 
wheels have been studied in [15], [16], and several optimal 
control problems for multibody systems have been studied 
in [17], [8]. In addition to geometric phase, connected rigid 
bodies can steer themselves by exchanging linear momentum 
and angular momentum with the fluid. 



C. Computational Approach 

We apply a direct optimal control approach. The control 
inputs are parameterized by several points that are uni- 
formly distributed over the maneuver time, and control inputs 
between these points are approximated using cubic spline 
interpolation. For given control input parameters, the value 
of the cost is given by (IZST i, and the terminal conditions are 
obtained by the discrete-time equations of motion (fT8Tl-(l22]). 
The control input parameters are optimized using constrained 
nonlinear parameter optimization to satisfy the terminal 
boundary conditions while minimizing the cost. 

This approach is computationally efficient when compared 
to the usual collocation methods, where the continuous-time 
equations of motion are imposed as constraints at a set of 
collocation points. Using the proposed discrete-time optimal 
control approach, optimal control inputs can be obtained 
by using a large step size, thereby resulting in efficient 
total computations. Since the computed optimal trajectories 
do not have numerical dissipation caused by conventional 
numerical integration schemes, they are numerically more 
robust. Furthermore, the corresponding gradient information 
of the cost and the terminal constraints with respect to the 
control parameters is accurately computed, which improves 
the convergence properties of the numerical optimization 
procedure. 

IV. Numerical Example 

The properties of connected rigid bodies are chosen as 
follows. The principal axes of each ellipsoid are given by 

Body 0:^1 =8, ^2 = 1-5, ^3 2 (m), 
Body 1,2: h = 5, 0.8, ^3 1.5 (m). 

We assume the density of the fluid is p = Ikg/m^. The 
corresponding inertia matrices are given by 

Mq = diag[1.0659, 2.1696, 1.6641], (kg) 
Ml = M2 = diag[0.2664, 0.6551, 0.3677] (kg), 
Jo = diag[1.3480, 20.1500, 25.3276] (kgm^), 
Ji = J2 = diag[0.1961, 1.7889, 2.9210] (kgm^). 

The location of the ball joints with respect to the center of 
mass of each body are chosen as 

doi = -do2 - [8.8, 0, 0], dio = -^20 = [5.5, 0, 0] (m). 
The initial configuration is as follows: 

Roo^Rio=I^ -R20 = exp (^^63^ , a;o = 03xi(m). 
The initial velocities are set to zero, i.e io,rioj^ij^2 — 

03X1- 

We consider the following two rest-to-rest maneuvers. 

(i) Forward translation along the ei axis 

ei • xjv = 2, RlR,^ = I for i e {0, 1, 2}. 

(ii) 180° rotation about the ei axis 

Ri„ ^ exp(7rei)i?io for i e {0, 1, 2}. 




(a) Snapshots at each 0.7 second 
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(b) Velocity of the central body (first 
and second component (m/s)) 



(c) Angular velocity (third compo- 
nents, rediQi, blue:f2o, green:f22, 
(rad/s)) 
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(d) Linear momentum along the (e) Control moment (third compo- 
ei axis (dashedirigid bodies, dot- nents, red:ui, green:M2, (kNm)) 
ted:fluid, solid:total (kgm/s)) 

Fig. 2. Optimal forward translation 



In every case, the terminal velocities are set to zero, and 
the unspecified parts of the terminal position are free. The 
maneuver time is T = 1 second, and the time step is 
h ~ 0.001 second. The first case is a planar motion, where 
the connected rigid bodies move in the 6162 plane, and the 
control moments ui,U2 act along the 63 axis. The second 
case is a three-dimensional maneuver. 

Fig. ID shows the optimal forward translation maneuver 
(an animation illustrating this maneuver is available at 
http : //my . fit . edu/^taeyoung). The linear momentum 
exchange between rigid bodies and fluid along the ei axis 
is illustrated at Fig. |2(d)| While the zero value of the total 
linear momentum is preserved throughout the maneuver, the 
average value of the linear momentum of the rigid bodies 
is positive. Therefore, the system of connected rigid bodies 
moves forward. 

Fig. [3] illustrates the optimal rotation maneuver. It is inter- 
esting to observe that the optimal velocity, angular velocity, 
and control inputs are almost symmetric about the mid- 
maneuver time t = 0.5. Similar to the optimal translation 
maneuver, at Fig. |3(d)[ the total angular momentum is 
preserved, but the average angular momentum of the rigid 
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(a) Snapshots at each 0.7 second (top view and back view) 
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(b) Velocity of the central body (each (c) Angular velocity (each compo- 



component (in/s)) 



nent, red:f2i, blue:f2o, green:C2, 
(rad/s)) 




(d) Angular momentum about the (e) Control moment (each compo- 
ei axis (dahsed:rigid bodies, dot- nent, redmi, green:u2, (kNm)) 
ted:fluid, solidlotal (kgm/s)) 

Fig. 3. Optimal 180° rotation about the e\ axis 



bodies about the rotation axis is positive. In addition to 
the angular momentum exchange between the rigid bodies 
and the fluid, this rotational maneuver is achieved by the 
geometric phase effect for coupled rigid bodies [18]. 

V. Conclusion 

For both cases, the proposed computational geometric op- 
timal control approach successfully finds nontrivial optimal 
maneuvers of connected rigid bodies in a perfect fluid. These 
optimal maneuvers are based on the nonlinear coupling 
between the shape space and the group motions, and they 
involve large-angle rotations of connected rigid bodies. 

The computational framework described in this paper ex- 
tends the class of computationally tractable shape-based fish 
locomotion models to include three-dimensional articulated 
multibody systems, thereby allowing for the study of optimal 
shape maneuvers for fish movement patterns beyond that of 
carangiform locomotion. 
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